#include "LinAlg.h"
#include <stdio.h>

Vector4::Vector4(){
x=0.0; y=0.0; z=0.0; q=0.0;
}

Vector4::Vector4(Vector4* v){
x = v->get('x');
y = v->get('y');
z = v->get('z');
q = v->get('w');
}

Vector4::Vector4(float x0,float y0,float z0, float q0){
x=x0; y=y0; z=z0; q=q0;
}

Vector4::Vector4(float &x0,float &y0,float &z0, float &q0){
x=x0; y=y0; z=z0; q=q0;
}

void Vector4::set(float x0,float y0,float z0, float q0){
x=x0; y=y0; z=z0; q=q0;
}

void Vector4::set(Vector4 b){
x=b.x; y=b.y; z=b.z; q=b.q;
}

void Vector4::add(Vector4 &a){
x+=a.x; y+=a.y; z+=a.z; q+=a.q;
}

void Vector4::add(Vector4 &a,Vector4 &b){
x=a.x+b.x; y=a.y+b.y; z=a.z+b.z; q=a.q+b.q;
}

void Vector4::subtract(Vector4 &a){
x-=a.x; y-=a.y; z-=a.z; q-=a.q;
}

void Vector4::subtract(Vector4 &a,Vector4 &b){
x=a.x-b.x; y=a.y-b.y; z=a.z-b.z; q=a.q-b.q;
}

void Vector4::negate(){
x=-x; y=-y; z=-z; q=-q;
}

void Vector4::negate(Vector4 &a){
x=-a.x; y=-a.y; z=-a.z; q=-a.q;
}

void Vector4::scale(float s){
x*=s; y*=s; z*=s; q*=s;
}

void Vector4::scale(float s,Vector4 &a){
x=s*a.x; y=s*a.y; z=s*a.z; q=s*a.q;
}

float Vector4::dot(Vector4 &a){
return x*a.x + y*a.y + z*a.z + q*a.q;
}

float Vector4::magnitude(){
return sqrt(x*x+y*y+z*z);
}

void Vector4::normalize(){
scale(1.0/magnitude());
}

void Vector4::dehomoginize(){
x /= q;
y /= q;
z /= q;
q /= q;
}

float Vector4::get(char a){
if(a == 'x') return x;
if(a == 'y') return y;
if(a == 'z') return z;
if(a == 'w') return q;
return 0.0;
}

float* Vector4::get(){
float* res = (float*) malloc(4);
res[0] = x;
res[1] = y;
res[2] = z;
res[3] = q;
return res;
}

float Vector4::operator[](const int i){
return get()[i];
}

// vector addition operator overload
Vector4& Vector4::operator+(Vector4 &c){
Vector4 *res = new Vector4;
res->add(c, *this);
return *res;
}

// vector subtraction operator overload
Vector4& Vector4::operator-(Vector4 &c){
Vector4 *res = new Vector4;
res->subtract(*this, c);
return *res;
}

void Vector4::printVector(){
	printf("\tX: %.4f, Y: %.4f, Z: %.4f, W: %.2f\n",x,y,z,q);
}

